#include "rviz_util_pkg/rviz_marker.hpp"

namespace rviz_util_pkg
{
    RvizMarker::RvizMarker(rclcpp::Node *node)
    {
        // 创建发布者，发布 Marker 消息到 RViz
        marker_point_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_point", 2);
        marker_txt_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_txt",
            10);
        marker_line_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_line",
            10);
    }
    RvizMarker::RvizMarker(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
    {
        // 创建发布者，发布 Marker 消息到 RViz
        marker_point_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_point", 2);
        marker_txt_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_txt",
            10);
        marker_line_pub_ = node->create_publisher<visualization_msgs::msg::Marker>(
            "/rviz_util_pkg/marker_line",
            10);
    }
    RvizMarker::~RvizMarker()
    {
        marker_point_pub_.reset();
        marker_txt_pub_.reset();
        marker_line_pub_.reset();
    }

    void RvizMarker::publish_marker_point(double x, double y, std::string name)
    {
        RCLCPP_INFO(logger_, "发布点位 Publishing marker name =%s,x=%f, y=%f",
                    name.c_str(),
                    x, y);
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = global_frame_id_;
        marker.header.stamp = rclcpp::Clock().now();
        marker.ns = name;
        marker_point_id_++;
        marker.id = marker_point_id_;
        marker.type = visualization_msgs::msg::Marker::POINTS;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;
        marker.scale.x = 0.1;
        marker.scale.y = 0.1;
        marker.scale.z = 0.1;
        marker.color.a = 1.0; // Don't forget to set the alpha!
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        // point
        geometry_msgs::msg::Point point;
        point.x = x;
        point.y = y;

        marker.points.push_back(point);
        marker_point_pub_->publish(marker);
    }
    void RvizMarker::clear_ns_points(const std::string &ns_name)
    {
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = global_frame_id_;
        marker.header.stamp = rclcpp::Clock().now();
        marker.ns = ns_name;
        marker.action = visualization_msgs::msg::Marker::DELETEALL;
        marker_point_pub_->publish(marker);
    }
    void RvizMarker::publish_marker_txt(double x, double y, std::string name)
    {
        visualization_msgs::msg::Marker text_marker;
        text_marker.header.frame_id = global_frame_id_;
        text_marker.header.stamp = rclcpp::Clock().now();
        text_marker.ns = "text_markers";
        marker_txt_id_++;
        text_marker.id = marker_txt_id_;
        text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
        text_marker.action = visualization_msgs::msg::Marker::ADD;
        text_marker.pose.position.x = x;
        text_marker.pose.position.y = y;
        text_marker.pose.position.z = 0 + 0.2; // 文本稍微高一点
        text_marker.pose.orientation.x = 0.0;
        text_marker.pose.orientation.y = 0.0;
        text_marker.pose.orientation.z = 0.0;
        text_marker.pose.orientation.w = 1.0;
        text_marker.scale.z = 0.2; // 文本大小
        text_marker.color.a = 1.0; // 不透明度
        text_marker.color.r = 0.5; // 颜色红色
        text_marker.color.g = 0.5;
        text_marker.color.b = 0.5;
        text_marker.text = name;

        marker_txt_pub_->publish(text_marker);
    }

    void RvizMarker::publish_marker_line(double x1, double y1, double x2, double y2)
    {
        // 画线
        visualization_msgs::msg::Marker line_marker;
        line_marker.header.frame_id = global_frame_id_;
        line_marker.header.stamp = rclcpp::Clock().now();
        line_marker.ns = "connect_two_points";
        marker_line_id_++;
        line_marker.id = marker_line_id_;
        line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
        line_marker.action = visualization_msgs::msg::Marker::ADD;
        line_marker.pose.orientation.w = 1.0;
        line_marker.scale.x = 0.01;
        line_marker.color.a = 0.5;
        line_marker.color.r = 0.3;
        line_marker.color.g = 0.3;
        line_marker.color.b = 0.3;
        // point
        geometry_msgs::msg::Point point_1;
        point_1.x = x1;
        point_1.y = y1;
        geometry_msgs::msg::Point point_2;
        point_2.x = x2;
        point_2.y = y2;
        line_marker.points.push_back(point_1);
        line_marker.points.push_back(point_2);
        marker_line_pub_->publish(line_marker);
    }
}